#ifndef SROS_LOCATION_PREDICT_IMPL_HPP
#define SROS_LOCATION_PREDICT_IMPL_HPP

#include <commons/impl/noncopyable.hpp>
#include <commons/sync_async/async_timer.hpp>

#include <Eigen/Dense>

#include <atomic>
#include <memory>
#include <mutex>

namespace sros::location
{
    template<typename XT, typename FT, typename PT, typename QT, typename BT, typename UT>
    class predict_impl : public impl::noncopyable
    {
    public:
        struct Param 
        {
            XT X_;      // 状态向量
            FT F_;      // 状态转移矩阵
            PT P_;      // 误差协方差
            QT Q_;      // 过程噪声
            BT B_;      // 控制矩阵
            UT U_;      // 控制输入
            double dt_; // delta time  (s)
        };

        predict_impl() : impl::noncopyable()
        {
            init_();
        }

        virtual ~predict_impl() = default;

        void run()
        {
            std::lock_guard<std::mutex> lck(create_mtx_);
        
            if (cycle_timer_)
            {
                return;
            }

            cycle_timer_ = std::make_shared<async::Timer>();
            cycle_timer_->safe_start(10, param_->dt_*1000, 0, [this](std::size_t)
            {
                std::lock_guard<std::mutex> lck(data_mtx_);

                param_->X_ = param_->F_ * param_->X_ + param_->B_ * param_->U_;
                param_->P_ = param_->F_ * param_->P_ * param_->F_.transpose() + param_->Q_;
            });
        }

        void close()
        {
            std::lock_guard<std::mutex> lck(create_mtx_);
            if (cycle_timer_)
            {
                cycle_timer_->safe_stop();
                cycle_timer_->safe_clear();
                cycle_timer_ = nullptr;
            }
        }

        void set_param(const Param &param)
        {
            std::lock_guard<std::mutex> lck(data_mtx_);
            param_ = std::make_shared<Param>(param);
        }

        void set_Q(const QT &q)
        {
            std::lock_guard<std::mutex> lck(data_mtx_); 
            param_->Q_ = q;
        }

        void set_U(const UT &u)
        {
            std::lock_guard<std::mutex> lck(data_mtx_); 
            param_->U_ = u;
        }

        void set_X(const XT &x)
        {
            std::lock_guard<std::mutex> lck(data_mtx_); 
            param_->X_ = x;
        }

        void set_P(const PT &p)
        {
            std::lock_guard<std::mutex> lck(data_mtx_); 
            param_->P_ = p;
        }

        struct Param param()
        {
            std::lock_guard<std::mutex> lck(data_mtx_);
            return *param_;
        }

    protected:
        virtual void init_()
        {
            param_ = std::make_shared<Param>();
            cycle_timer_ = nullptr;
        }

    protected:         
        std::mutex                    create_mtx_;
        std::mutex                    data_mtx_;
        std::shared_ptr<async::Timer> cycle_timer_;
        std::shared_ptr<struct Param> param_;       // 计算参数
    };

}

#endif